/*
 * Car.cpp
 *
 *  Created on: 21/11/2009
 *      Author: Facu
 */
#include "map/car/Car.h"

namespace map {

Car::Car(int aceleration, int maxSpeed, double initialAngle, double xPosition, double yPosition):
	position(xPosition, yPosition) {
	this->speed = 0;
	this->angle = initialAngle;
	this->maxSpeed = maxSpeed;
	this->aceleration = aceleration;
}

Car::~Car() {
}

void Car::accelerate() {
	if (this->speed < this->maxSpeed) {
		if ((this->speed + this->aceleration) > this->maxSpeed) {
			this->speed = this->maxSpeed;
		} else {
			this->speed += this->aceleration;
		}
	}
}

void Car::decelerate() {
	if (this->speed > 0) {
		if ((this->speed - this->aceleration) < 0) {
			this->speed = 0;
		} else {
			this->speed -= this->aceleration;
		}
	}
}

void Car::stop() {
	this->speed = 0;
}

void Car::simplifyAngle() {
	if (util::doubleEquals(this->angle, 2 * PI) || util::doubleEquals(this->angle, -2 * PI)) {
		this->angle = 0;
	}
}
void Car::turn(double angle) {
//	this->angle = angle;
	this->angle = util::correctAnglePhase(this->angle - angle);
}
void Car::turnLeft() {
	this->angle += PI / 2.0;
	this->simplifyAngle();
}

void Car::turnRight() {
	this->angle -= PI / 2.0;
	this->simplifyAngle();
}

int Car::getSpeed() {
	return this->speed;
}

double Car::getAngle() {
	return this->angle;
}

void Car::setPosition(MapPoint position){
    this->position = position;
}

MapPoint Car::getPosition(){
	return this->position;
}

void Car::setAngle(double angle){
    this->angle = angle;
}


int Car::getMaxSpeed(){
	return this->maxSpeed;
}
double Car::advance(int deltaT){
	double metersPerMilisecond = (this->getSpeed() * 1000) / 3600000.0;
	return metersPerMilisecond * deltaT;
}

}
